#include "Sw64.h"

SW64_FLASH_MEMMAP Swflash = {0};
int CpuNum = 0;

static SW64_FLASH_MEMMAP* get_VirtAddr_from_context(const struct flashctx *flash)
{
	if (!flash || !flash->mst || !flash->mst->spi.data) {
		msg_perr("Unable to extract Virt Addr from flash context.\n");
		return NULL;
	}
	return flash->mst->spi.data;
}

static VOID WriteSER (UINT32 Data)
{
  UINT32 SRReg;
  UINT8 SERReg;

  SRReg = MmioRead32 (Swflash.spi_ctrl_reg_base_virt + SR) ;
  while ((SRReg & 0x01) == 1)
  {
    SRReg =  MmioRead32 (Swflash.spi_ctrl_reg_base_virt + SR);
  }
  SERReg = MmioRead32 (Swflash.spi_ctrl_reg_base_virt + SER);
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + SER, ((SERReg & 0xFE) | Data));
  return;
}

static int Sw64_spi_send_command(const struct flashctx *flash,
				unsigned int writecnt, unsigned int readcnt,
				const unsigned char *writearr,
				unsigned char *readarr)
{
    uint32_t i;


	if (writecnt > 9 || readcnt > SPI_MAX_READ_NUM || writecnt == 0) {
		msg_perr("%s: Invalid read/write count for send command.\n",
                         __func__);
		return SPI_GENERIC_ERROR;
	}

	  SW64_FLASH_MEMMAP *pVirtAddr = get_VirtAddr_from_context(flash);
    if (NULL == pVirtAddr)
		return SPI_GENERIC_ERROR;

    MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt  + SSIENR, 0x00);
    if (readcnt)
    {
        MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt  + CTRLR0, 0x3c7);
        MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt  + CTRLR1, readcnt - 1);
    }
    else
    {
        MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt + CTRLR0, 0x1c7);
    }

    MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt + SSIENR, 0x1);

    WriteSER(0);
    for(i = 0; i < writecnt; i++){
      MmioWrite32 (pVirtAddr->spi_ctrl_reg_base_virt + DR, writearr[i]);
     // msg_perr("write  %x %x.\n", writearr[i], i);
    }

    WriteSER(1);
    //wait for the TX FIFO empty
    while(MmioRead32 (pVirtAddr->spi_ctrl_reg_base_virt + TXFLR) != 0);
    while(MmioRead32 (pVirtAddr->spi_ctrl_reg_base_virt + SR) == 1);

    
    while (MmioRead32 (pVirtAddr->spi_ctrl_reg_base_virt + RXFLR) < readcnt);

    for (i = 0; i < readcnt; i++) {
      readarr[i] = (UINT8)(MmioRead32 (pVirtAddr->spi_ctrl_reg_base_virt + DR));
    }

    WriteSER(0);

    return 0;
}

#define EXADDR          0xC5
static void spi_pp_cfg(struct flashctx *flash, int addr)
{

    unsigned char writearr[2] = {0x06, 0x00};
    if (flash->address_high_byte != addr)
    {
        Sw64_spi_send_command(flash, 1, 0, writearr, NULL);
        writearr[0] = flash->chip->wrea_override ? flash->chip->wrea_override : EXADDR;
        writearr[1] = addr;
        Sw64_spi_send_command(flash, 2, 0, writearr, NULL);
        flash->address_high_byte = addr;
    }
}

static int Sw64_spi_read(struct flashctx *flash, uint8_t *buf,
			unsigned int start, unsigned int len)
{
    uint32_t i = 0;
    char cmdbuf[5] = {0};
    cmdbuf[0] = 0x03;
    unsigned int Src = 0;
    unsigned int readlen = 0;

    spi_pp_cfg(flash, (start >> 24));

    for (i = 0; i < len; i+=SPI_MAX_READ_NUM)
    {
        if ((i + SPI_MAX_READ_NUM) > len)
        {
            readlen = len - i;
        }
        else
        {
            readlen = SPI_MAX_READ_NUM;
        }

        Src = start + i;
        cmdbuf[1] = (Src>>16)&0xFF;
        cmdbuf[2] = (Src>>8)&0xFF;
        cmdbuf[3] = (Src)&0xFF;

        //memcpy(&cmdbuf[1], (void *)&Src, 4);

        Sw64_spi_send_command(flash, 4, readlen, (const unsigned char *)cmdbuf, buf + i);
    }
    
    return 0;
}

static int Sw64_spi_write_256(struct flashctx *flash, const uint8_t *buf,
				unsigned int start, unsigned int len)
{
  unsigned int i = 0;
  char cmd = 0x00;
  char cmdbuf[9] = {0};
  unsigned char status = 0;
  unsigned int dest = 0;

  spi_pp_cfg(flash, (start >> 24));

  for (i = 0; i < len; i+=4)
  {
      cmd = 0x06;
      Sw64_spi_send_command(flash, 1, 0, (const unsigned char *)&cmd, NULL);

      cmd = 0x05;
      Sw64_spi_send_command(flash, 1, 1, (const unsigned char *)&cmd, &status);
      while ((status & 0x02) == 0){
        Sw64_spi_send_command(flash, 1, 1, (const unsigned char *)&cmd, &status);
      }

      cmdbuf[0] = 0x02;
      dest = start + i;
      cmdbuf[1] = (dest>>16)&0xFF;
      cmdbuf[2] = (dest>>8)&0xFF;
      cmdbuf[3] = (dest)&0xFF;
      memcpy(&cmdbuf[4], (void *)(buf + i), 4);
      Sw64_spi_send_command(flash, 8, 0, (const unsigned char *)&cmdbuf, NULL);

      cmd = 0x05;
      Sw64_spi_send_command(flash, 1, 1, (const unsigned char *)&cmd, &status);
      while ((status & 0x03)){
        Sw64_spi_send_command(flash, 1, 1, (const unsigned char *)&cmd, &status);
      }
  }
  

	return 0;
}

static int Sw64_spi_write_aai(struct flashctx *flash, const uint8_t *buf,
				 unsigned int start, unsigned int len)
{
	msg_perr("%s: Not Support.\n", __func__);

	return 0;
}

static int Sw64_spi_shutdown(void *data)
{
    SW64_FLASH_MEMMAP *pVirtAddr = data;

    if (pVirtAddr->spi_ctrl_reg_base_virt)
    {
        physunmap (pVirtAddr->spi_ctrl_reg_base_virt, SPI_REG_LENGTH);
        pVirtAddr->spi_ctrl_reg_base_virt = NULL;
    }

    if (pVirtAddr->csr_ctrl_reg_base_virt)
    {
        physunmap (pVirtAddr->csr_ctrl_reg_base_virt, CSR_REG_LENGTH);
        pVirtAddr->csr_ctrl_reg_base_virt = NULL;
    }

	return 0;
}

static const struct spi_master spi_master_SW64 = {
	.max_data_read = 0xFFFF,
	.max_data_write = SPI_MAX_READ_NUM,
	.command = Sw64_spi_send_command,
	.multicommand = default_spi_send_multicommand,
	.read = Sw64_spi_read,
	.write_256 = Sw64_spi_write_256,
	.write_aai = Sw64_spi_write_aai,
	.shutdown = Sw64_spi_shutdown,
};

static void *NullMap(const char *descr, uintptr_t phys_addr, size_t len)
{
    return Swflash.spi_ctrl_reg_base_virt;
}

static void NullUnMap(void *virt_addr, size_t len)
{
    return;
}

static VOID SpiFlashInit (VOID)
{
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + SSIENR, 0x00);
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + BAUDR, 0x2);
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + IMR, 0x2e);
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + RXFTLR, 0xFF);//jd add
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + TXFTLR, 0xff);
  MmioWrite32 (Swflash.spi_ctrl_reg_base_virt + SSIENR, 0x1);
}

static int SW64_init(void)
{
    char *arg;

    arg = extract_programmer_param("CPU");
    if (arg) {
      CpuNum = strtol(arg, NULL, 10);
    } else if (arg && !strlen(arg)) {
      msg_perr("Missing argument for CPU.\n");
      free(arg);
      return 1;
    } 

    if (arg)
        free(arg);

    Swflash.spi_ctrl_reg_base_virt = physmap("SPICtrl", (SPI_CTRL_REG_BASE + CpuNum * SPI_CPU_INTERVAL), SPI_REG_LENGTH);
    Swflash.csr_ctrl_reg_base_virt = physmap("CSR", CSR_CTRL_REG_BASE, CSR_REG_LENGTH);
    MmioWrite32 (Swflash.csr_ctrl_reg_base_virt + SPI_SRST_L, 0x00);
    internal_delay(100000);
    MmioWrite32 (Swflash.csr_ctrl_reg_base_virt + SPI_SRST_L, 0x01);

    SpiFlashInit();
    return register_spi_master(&spi_master_SW64, &Swflash);
}

const struct programmer_entry programmer_SW64 = {
	.name			= "SW64",
	.type			= OTHER,
	.devs.note		= "SW64.\n",
	.init			= SW64_init,
	.map_flash_region	= NullMap,
	.unmap_flash_region	= NullUnMap,
	.delay			= internal_delay,
};